/**
 * 
 */
package SRTPlanner;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.Set;

import simulation.ConfigGraph;
import simulation.Configuration;
import simulation.LineSeg;
import simulation.Map;
import simulation.Obstacle;
import simulation.Rect;
import simulation.Robot;
import simulation.Simulation;

/**
 * @author aclayton
 *
 */
public class EST {
	private Simulation sim;
	private Rect rect;
	private SimpleLocalPlanner planner;
	private Map map;
	private ConfigGraph graph;
	private int currentIndex;
	private Configuration startConfig;
	
	public EST(Simulation sim, Rect subRect, Configuration estStartConfig){
		this.sim = sim;
		this.map = sim.getMap();
		this.rect = subRect;
		this.planner = new SimpleLocalPlanner(this.map);
		currentIndex = 0;
		graph  = new ConfigGraph(subRect);
		startConfig = estStartConfig;
		graph.addConfiguration(estStartConfig);
	}
	
	public void addToTree(Configuration c){
		graph.addConfiguration(c);
	}
	
	
	public void addRandomConfigurations(int numConfigs){
		
		for(int a = 0; a < numConfigs; a++){
			Configuration c = Configuration.CreateRandomConfig(sim, rect);
			if(!c.inFreeSpace(map))
				continue;
			
			HashSet<Configuration> nearest = graph.getNearestConfigsDistance(c, 50);
			for(Configuration near : nearest){
				if(planner.existsPath(c, near)){
					graph.addConfiguration(c);
					graph.addEdge(c, near);
				}
			}
		}
		/*
		if(end != null){
			Set<Configuration> configs = graph.getConfigurations();
			
			for(Configuration c : configs){
				if(planner.existsPath(c, end))
					graph.addEdge(c, end);
			}
		}*/
	}
	
	public ConfigGraph getGraph(){
		return graph;
	}

}
